// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; version 2 of the License.

// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.

#include "EllipticalOrbit.h"

#include <functional>
#include <cmath>

using namespace std;

#ifndef PI
#define PI 3.141592653589793238462643383279502884
#endif

EllipticalOrbit::EllipticalOrbit(double _pericenterDistance, double _eccentricity, double _inclination, double _ascendingNode,
							   double _argOfPeriapsis, double _meanAnomalyAtEpoch, double _period, double _epoch) : pericenterDistance(_pericenterDistance), eccentricity(_eccentricity), inclination(_inclination), ascendingNode(_ascendingNode), argOfPeriapsis(_argOfPeriapsis), meanAnomalyAtEpoch(_meanAnomalyAtEpoch), epoch(_epoch)

{
	//parent = _parent;
	period = _period;
	orbitPlaneRotation = (Mat3::zrotation(_ascendingNode) *
                          Mat3::xrotation(_inclination) *
                          Mat3::zrotation(_argOfPeriapsis));

}

template<class T> T sign(T x)
{
    if (x < 0)
        return -1;
    else if (x > 0)
        return 1;
    else
        return 0;
}

template<class T> T square(T x)
{
    return x * x;
}


struct SolveKeplerFunc1 : public unary_function<double, double>
{
    double ecc;
    double M;

    SolveKeplerFunc1(double _ecc, double _M) : ecc(_ecc), M(_M) {};

    double operator()(double x) const
    {
        return M + ecc * sin(x);
    }
};

struct SolveKeplerFunc2 : public unary_function<double, double>
{
    double ecc;
    double M;

    SolveKeplerFunc2(double _ecc, double _M) : ecc(_ecc), M(_M) {};

    double operator()(double x) const
    {
        return x + (M + ecc * sin(x) - x) / (1 - ecc * cos(x));
    }
};

struct SolveKeplerLaguerreConway : public unary_function<double, double>
{
    double ecc;
    double M;

    SolveKeplerLaguerreConway(double _ecc, double _M) : ecc(_ecc), M(_M) {};

    double operator()(double x) const
    {
        double s = ecc * sin(x);
        double c = ecc * cos(x);
        double f = x - s - M;
        double f1 = 1 - c;
        double f2 = s;
        x += -5 * f / (f1 + sign(f1) * sqrt(abs(16 * f1 * f1 - 20 * f * f2)));

        return x;
    }
};

struct SolveKeplerLaguerreConwayHyp : public unary_function<double, double>
{
    double ecc;
    double M;

    SolveKeplerLaguerreConwayHyp(double _ecc, double _M) : ecc(_ecc), M(_M) {};

    double operator()(double x) const
    {
        double s = ecc * sinh(x);
        double c = ecc * cosh(x);
        double f = s - x - M;
        double f1 = c - 1;
        double f2 = s;
        x += -5 * f / (f1 + sign(f1) * sqrt(abs(16 * f1 * f1 - 20 * f * f2)));

        return x;
    }
};

typedef pair<double, double> Solution;


template<class T, class F> std::pair<T, T> solve_iteration_fixed(F f,
                                                                 T x0,
                                                                 int maxIter)
{
    T x = 0;
    T x2 = x0;

    for (int i = 0; i < maxIter; i++)
    {
        x = x2;
        x2 = f(x);
    }

    return std::make_pair(x2, x2 - x);
}


double EllipticalOrbit::eccentricAnomaly(double M) const
{
    if (eccentricity == 0.0)
    {
        // Circular orbit
        return M;
    }
    else if (eccentricity < 0.2)
    {
        // Low eccentricity, so use the standard iteration technique
        Solution sol = solve_iteration_fixed(SolveKeplerFunc1(eccentricity, M), M, 5);
        return sol.first;
    }
    else if (eccentricity < 0.9)
    {
        // Higher eccentricity elliptical orbit; use a more complex but
        // much faster converging iteration.
        Solution sol = solve_iteration_fixed(SolveKeplerFunc2(eccentricity, M), M, 6);
        // Debugging
        // printf("ecc: %f, error: %f mas\n",
        //        eccentricity, radToDeg(sol.second) * 3600000);
        return sol.first;
    }
    else if (eccentricity < 1.0)
    {
        // Extremely stable Laguerre-Conway method for solving Kepler's
        // equation.  Only use this for high-eccentricity orbits, as it
        // requires more calcuation.
        double E = M + 0.85 * eccentricity * sign(sin(M));
        Solution sol = solve_iteration_fixed(SolveKeplerLaguerreConway(eccentricity, M), E, 8);
        return sol.first;
    }
    else if (eccentricity == 1.0)
    {
        // Nearly parabolic orbit; very common for comets
        // TODO: handle this
        return M;
    }
    else
    {
        // Laguerre-Conway method for hyperbolic (ecc > 1) orbits.
        double E = log(2 * M / eccentricity + 1.85);
        Solution sol = solve_iteration_fixed(SolveKeplerLaguerreConwayHyp(eccentricity, M), E, 30);
        return sol.first;
    }
}

Vec3 EllipticalOrbit::positionAtE(double E) const
{
    double x, y;

    if (eccentricity < 1.0)
    {
        double a = pericenterDistance / (1.0 - eccentricity);
        x = a * (cos(E) - eccentricity);
        y = a * sqrt(1 - square(eccentricity)) * sin(E);
    }
    else if (eccentricity > 1.0)
    {
        double a = pericenterDistance / (1.0 - eccentricity);
        x = -a * (eccentricity - cosh(E));
        y = -a * sqrt(square(eccentricity) - 1) * sinh(E);
    }
    else
    {
        // TODO: Handle parabolic orbits
        x = 0.0;
        y = 0.0;
    }

    //VECTOR p(-1.*x, -1.*y, 0);

	Vec3 p = orbitPlaneRotation * Vec3(x, y, 0);
	
	//p.x *= -1.;
	//p.y *= -1.;

    // Convert to Celestia's internal coordinate system
    return p;
}

VECTOR EllipticalOrbit::computePosition(double jd)
{
    jd = jd - epoch;
    double meanMotion = 2.0 * PI / period;
    double meanAnomaly = meanAnomalyAtEpoch + jd * meanMotion;
    double E = eccentricAnomaly(meanAnomaly);

    VECTOR pos = positionAtE(E);

	return pos;
}

EllipticalOrbit::~EllipticalOrbit(void)
{
}
